#include "CoordinateProcessor.h"

#include <iostream>

#include "DataStructure/Common/Constant.h"
#include "DataStructure/Input/Coordinate/CoordTypes.h"
#include "DataStructure/Input/Node/NodeData.h"

#include "DBManager/IService/Input/Coordinate/ICORDSummaryService.h"
#include "DBManager/IService/Input/Node/INodeService.h"

#include "SGFem/Pre/Assembly/Assembly.h"

using SG::DataStructure::Common::Status;
using SG::Algebra::Matrix;
using SG::Algebra::Point;
using SG::Algebra::Distance;
using SG::DataStructure::Common::Real;
using SG::DataStructure::FEM::CoordStatus;
using SG::DataStructure::FEM::CoordTypes;
using SG::DataStructure::FEM::ICoordinate;
using SG::DataStructure::FEM::ICoordinateP;
using SG::DataStructure::FEM::ICoordinatePArray;
using SG::DataStructure::Common::Id;
using SG::DataStructure::FEM::NodeData;

using namespace SG::DBManager;
using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::CoordinateProcessor;

void CoordinateProcessor::ReadCoord (DBServiceFactorySP dbServiceFactory, ICoordinatePArray& _OUT coordSet)
{
    const AssemblyEigen& assembly = AssemblyEigen::GetInstance ();

    auto coordArr = dbServiceFactory->get<ICORDSummaryService> ()->GetAllCoordinate ();

    coordSet.clear ();

    // 依次创建各个坐标系
    for (std::shared_ptr<SG::DataStructure::FEM::ICoordinate> coord : coordArr)
    {
        // 通过节点定义的坐标系, 额外指定节点的参考坐标系
        if (CoordTypes::COORDTYPE_Cartesian == coord->m_type || CoordTypes::COORDTYPE_Cylinder == coord->m_type
            || CoordTypes::COORDTYPE_Sphere == coord->m_type)
        {
            Id          GA, GB, GC;
            std::size_t index;
            NodeData    P;
            coord->GetRefPoint (GA, GB, GC);
            // 节点A的定义坐标系
            assembly.FindNodeIndex (GA, _OUT index);
            dbServiceFactory->get<INodeService> ()->FindByIndex (index, _OUT P);
            coord->SetRefCoordId (0, P.m_defCoord);

            // 节点B的定义坐标系
            assembly.FindNodeIndex (GB, _OUT index);
            dbServiceFactory->get<INodeService> ()->FindByIndex (index, _OUT P);
            coord->SetRefCoordId (1, P.m_defCoord);

            // 节点C的定义坐标系
            assembly.FindNodeIndex (GC, _OUT index);
            dbServiceFactory->get<INodeService> ()->FindByIndex (index, _OUT P);
            coord->SetRefCoordId (2, P.m_defCoord);
        }

        coordSet[coord->m_id] = coord;
    }
}

void CoordinateProcessor::Compute (const DBServiceFactorySP dbServiceFactory, ICoordinatePArray& _OUT coordSet)
{
    ReadCoord (dbServiceFactory, coordSet);

    const AssemblyEigen& assembly = AssemblyEigen::GetInstance ();
    const auto& nodeRepository = dbServiceFactory->get<INodeService> ();
    // 坐标系数量
    auto nCoord = coordSet.size ();
    // 不存在坐标系定义
    if (0 == nCoord)
        return;

    auto                                undeterminedLast = nCoord;  // 上一轮没有确定的坐标系数量
    SG::DataStructure::FEM::CoordStatus status;
    while (1)
    {
        std::size_t nInitCoord          = 0;
        std::size_t undeterminedCurrent = 0;
        for (auto& coord : coordSet)
        {
            // 坐标系是否已经决定
            auto& it = coord.second;

            status = it->QueryStatus (coordSet);
            // 之前循环轮， 已经处理
            if (CoordStatus::Status_Initialized == status)
            {
                ++nInitCoord;
                continue;
            }

            // 到本轮， 被依赖的坐标系已经处理， 计算当前坐标
            if (CoordStatus::Status_Ready == status)
            {
                Point                    PA, PB, PC;
                std::vector<std::size_t> nodeIndexes (3);
                const auto               type = it->m_type;
                if (type == CoordTypes::COORDTYPE_Cartesian || type == CoordTypes::COORDTYPE_Cylinder
                    || type == CoordTypes::COORDTYPE_Sphere)
                {
                    SG::DataStructure::Common::Id Gi[3];
                    // 查询定义节点Id
                    it->GetRefPoint (_OUT Gi[0], _OUT Gi[1], _OUT Gi[2]);

                    // 获取节点数据
                    assembly.FindNodeIndex (Gi, _OUT nodeIndexes);
                    NodeData gridA, gridB, gridC;
                    nodeRepository->FindByIndex (nodeIndexes[0], _OUT gridA);
                    nodeRepository->FindByIndex (nodeIndexes[1], _OUT gridB);
                    nodeRepository->FindByIndex (nodeIndexes[2], _OUT gridC);

                    // 将坐标变换到 Basic 坐标系下
                    PA = { gridA.m_x[0], gridA.m_x[1], gridA.m_x[2] };
                    if (0 != gridA.m_defCoord)
                    {
                        PA = coordSet[gridA.m_defCoord]->Trans2Basic (PA.m_x, PA.m_y, PA.m_z);
                    }
                    PB = { gridB.m_x[0], gridB.m_x[1], gridB.m_x[2] };
                    if (0 != gridB.m_defCoord)
                    {
                        PB = coordSet[gridB.m_defCoord]->Trans2Basic (PB.m_x, PB.m_y, PB.m_z);
                    }
                    PC = { gridC.m_x[0], gridC.m_x[1], gridC.m_x[2] };
                    if (0 != gridC.m_defCoord)
                    {
                        PC = coordSet[gridC.m_defCoord]->Trans2Basic (PC.m_x, PC.m_y, PC.m_z);
                    }
                }
                else
                {
                    Id ref;
                    it->GetRefPoint (_OUT ref, _OUT PA, _OUT PB, _OUT PC);

                    // 节点坐标变换到Basic 坐标系， 此时 ref坐标系的原点和基矢量已经确定
                    if (0 != ref)
                    {
                        PA = coordSet[ref]->Trans2Basic (PA.m_x, PA.m_y, PA.m_z);
                        PB = coordSet[ref]->Trans2Basic (PB.m_x, PB.m_y, PB.m_z);
                        PC = coordSet[ref]->Trans2Basic (PC.m_x, PC.m_y, PC.m_z);
                    }
                }

                // 计算坐标系基矢量
                it->Compute (PA, PB, PC);
                ++nInitCoord;
                continue;
            }
            // 其他，当前坐标系结果无法确定
            ++undeterminedCurrent;
        }  // for

        /**
         *  一轮变量坐标系循环结束， 可能出现三种可能性
         *     1. 所有坐标系都被确定，跳出循环
         *     2. 本轮循环没有新的坐标系能够被确定（即坐标系依赖关系出现环状），计算以失败告终
         *     3. 本轮坐标遍历，有新的坐标系确定了基矢量和原点。 但仍然具有坐标系还没有确定
         *        【可能依赖本次循环中新决定的坐标系】， 需要再次对所有坐标系进行遍历
         */
        // case 1 : 所有坐标系都确定了
        if (nInitCoord == nCoord)
        {
            break;
        }

        // case 2: 无法产生新的消去
        if (undeterminedCurrent == undeterminedLast)
        {
            std::cout << "无法产生新的消去， 计算失败\n";
            return;
        }

        // case 3: 进行新一轮坐标系遍历
        // 更新没有被决定的坐标系数量
        undeterminedLast = undeterminedCurrent;
    }
}

CoordinateProcessor& CoordinateProcessor::GetInstance ()
{
    static CoordinateProcessor Instance;
    return Instance;
}

void CoordinateProcessor::Initialize (const SG::DBManager::DBServiceFactorySP dbServiceFactory)
{
    // 坐标系计算
    SG::DataStructure::FEM::ICoordinatePArray coordSet;
    Compute (dbServiceFactory, coordSet);
    this->SetCoordSet (coordSet);
}

/** 将计算好原点和基矢量的坐标系保存到本单例类的成员变量中*/
void CoordinateProcessor::SetCoordSet (const SG::DataStructure::FEM::ICoordinatePArray& coordSet)
{
    m_coordSet = coordSet;
}

/** @brief 通过坐标系编号，返回坐标系
 *
 * 对应坐标系数据不存在，将返回空指针。
 */
const SG::DataStructure::FEM::ICoordinateP CoordinateProcessor::GetCoord (SG::DataStructure::Common::Id id) const
{
    const auto it = m_coordSet.find (id);
    if (m_coordSet.end () == it)
    {
        return NULL;
    }
    return it->second;
}

void CoordinateProcessor::GetCoordTrans (const SG::DataStructure::Common::Id cId, const Point& point, _OUT Matrix& T) const
{
    auto coordSys = this->GetCoord (cId);
    assert (nullptr != coordSys);

    const auto  type   = coordSys->GetType ();
    const auto& origin = coordSys->GetOrigin ();
    /**
     * 当空间点球坐标系原点或者 柱坐标系Z轴，输出的基矢量取和坐标系原点定义基矢量相同
     */
    bool localInOrigin = (origin == point);

    // point 的坐标 是否恰好位于柱坐标系 z轴
    bool locateCyliderZAix{false};
    if (!localInOrigin)
    {
        if (type == CoordTypes::COORDTYPE_Cylinder || type == CoordTypes::COORDTYPE_Cylinder2)
        {
            auto OP = Distance (point, origin);
            Real tmp;
            OP.Normalize (tmp);
            const auto& zBase = coordSys->GetBaseZ ();
            Real dot = OP.m_x * zBase.m_x + OP.m_y * zBase.m_y + OP.m_z * zBase.m_z;
            if (fabs (fabs(dot) - 1.0) < 1.0e-6)
            {
                locateCyliderZAix = true;
            }
        }

    }
    // 笛卡尔坐标系，或者point 在柱坐标z轴上，或者空间点位于坐标系原点
    if (type == CoordTypes::COORDTYPE_Cartesian || type == CoordTypes::COORDTYPE_Cartesian2 || locateCyliderZAix || origin == point)
    {
        const auto& xBase = coordSys->GetBaseX ();
        const auto& yBase = coordSys->GetBaseY ();
        const auto& zBase = coordSys->GetBaseZ ();
        T = Matrix (3, 3, { xBase.m_x, xBase.m_y, xBase.m_z, yBase.m_x, yBase.m_y, yBase.m_z, zBase.m_x, zBase.m_y, zBase.m_z });
        return;
    }

    // 柱坐标系
    if (type == CoordTypes::COORDTYPE_Cylinder || type == CoordTypes::COORDTYPE_Cylinder2)
    {
        const auto& zBase = coordSys->GetBaseZ ();
        // 局部坐标原点到空间点矢量 r
        auto r = Algebra::Distance (point, origin);
        // r 往局部坐标系x-y平面投影
        r = r - zBase * (Algebra::dot (r, zBase));

        r.Normalize ();

        auto yBase = Algebra::Cross (zBase, r);

        T = Matrix (3, 3, { r.m_x, r.m_y, r.m_z, yBase.m_x, yBase.m_y, yBase.m_z, zBase.m_x, zBase.m_y, zBase.m_z });

        return;
    }

    // 球坐标系
    if (type == CoordTypes::COORDTYPE_Sphere || type == CoordTypes::COORDTYPE_Sphere2)
    {
        // 局部坐标原点到空间点矢量 r
        auto r = Algebra::Distance (point, origin);
        Real tmp;

        // ToDo: r 为零，需特殊处理，测试 Nastran 如何处理
        // If for cylindrical or spherical coordinate, GA falls on the z-axis used to define them,
        // it is recommended that another CID be selected to define the element x-axis.

        auto xBase = r.Normalize (tmp);

        const auto& z0    = coordSys->GetBaseZ ();
        auto        zBase = Algebra::Cross (z0, r);
        zBase.Normalize (tmp);

        auto yBase = Algebra::Cross (zBase, xBase);

        T = Matrix (3, 3, { xBase.m_x, xBase.m_y, xBase.m_z, yBase.m_x, yBase.m_y, yBase.m_z, zBase.m_x, zBase.m_y, zBase.m_z });

        return;
    }

    // 正常情况，函数不应该运行到此位置
    return;
}

/** @brief 将点转换到世界坐标系下
 *
 * @param cId    局部坐标系编号
 * @param point  节点在局部坐标系下坐标值
 * @param pNew   节点在世界坐标下分量
 */
void CoordinateProcessor::Translate2Basic (const SG::DataStructure::Common::Id cId,
                                           const Algebra::Point&            point,
                                           Algebra::Point& _OUT             pNew) const
{
    if (cId > 0)
    {
        const auto& coord = this->GetCoord (cId);
        assert (NULL != coord);

        pNew = coord->Trans2Basic (point.m_x, point.m_y, point.m_z);
    }
    else
    {
        pNew = point;
    }
}

/** @brief 计算向量在全局坐标下分量（不用进行偏移）
 *
 */
void CoordinateProcessor::Rotate2Basic (const SG::DataStructure::Common::Id cId, _OUT Algebra::Vector3D& vec) const
{
    if (cId > 0)
    {
        const auto& coord = this->GetCoord (cId);
        assert (NULL != coord);

        auto pNew   = coord->Trans2Basic (vec.m_x, vec.m_y, vec.m_z);
        auto origin = coord->GetOrigin ();
        vec         = Algebra::Distance (pNew, origin);
    }
}

void CoordinateProcessor::GetGlobalCoordinate (const SG::DataStructure::FEM::NodeData node,
                                               Algebra::Point& _OUT                   point,
                                               SG::DataStructure::Common::Id& _OUT       dispCoordId) const
{
    point.m_x = node.m_x[0];
    point.m_y = node.m_x[1];
    point.m_z = node.m_x[2];
    if (0 != node.m_defCoord)
    {
        this->Translate2Basic (node.m_defCoord, point, _OUT point);
    }

    dispCoordId = node.m_dispCoord;
}
